#include "ros/ros.h"
#include "turtlesim/Pose.h"
#include "tf2_ros/transform_broadcaster.h"
#include "geometry_msgs/TransformStamped.h"
#include "tf2/LinearMath/Quaternion.h"

void dopose(const turtlesim::Pose::ConstPtr &pose)
{
    //创建发布对象
    static tf2_ros::TransformBroadcaster pub;
    
    //组织被发布的数据
    geometry_msgs::TransformStamped ts;
    ts.header.frame_id = "world";
    ts.child_frame_id = "turtle1";
    ts.header.stamp = ros::Time::now();
        //坐标偏移量
    ts.transform.translation.x = pose -> x;
    ts.transform.translation.y = pose -> y;
    ts.transform.translation.x = 0;
        //设置四元数
    /*
            在位姿信息中没有四元数，只有偏航，但是乌龟是2d的，则只有z
    */
    tf2::Quaternion q;
    q.setRPY(0,0,pose -> theta);
    ts.transform.rotation.x = q.getX();
    ts.transform.rotation.y = q.getY();
    ts.transform.rotation.z = q.getZ();
    ts.transform.rotation.w = q.getW();

    //发布消息
    pub.sendTransform(ts);

}
int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"pub");
    ros::NodeHandle nh;

    //创建订阅对象
    ros::Subscriber sub = nh.subscribe("/turtle1/pose",100,dopose);


    ros::spin();
    return 0;
}
